#include <libut.h>
#include <sel4/sel4.h>
#include <stdlib.h>

#include "task.h"

#include "debug.h"
#define verbose SOS_DEBUG_TASK
#include <sys/debug.h>

/* The ready-list is managed by libut as a doubly-linked list */
static task_t _high;
static task_t _low;

int sos_task_add_ready(task_cb task, void *cookie, int priority) {
    dprintf(5, "task_add_ready: adding task\n");

    task_t newt = malloc(sizeof(struct task));
    if (newt == NULL) return SOS_TASK_OOM;
    newt->cb = task;
    newt->cookie = cookie;
    if (priority == SOS_TASK_PRIORITY_LOW) {
        DL_APPEND((_low), newt);
    } else {
        DL_APPEND((_high), newt);
    }
    dprintf(5, "task_add_ready: done\n");
    return SOS_TASK_SUCCESS;
}

static void _sos_task_run_ready(task_t *_ready) {
    dprintf(8, "Running ready queues...\n");
    task_t task = NULL, tmp = NULL;
    int err;
    DL_FOREACH_SAFE((*_ready), (task), (tmp)) {
        /* Run the callback, and delete from list if appropriate */
        dprintf(6, "task_run_ready: running task at %p\n", task);
        err = (*(task->cb))(task->cookie);
        if (err != SOS_TASK_YIELD) {
            if (err != SOS_TASK_SUCCESS) {
                /* Unknown error - print and destroy */
                dprintf(1, "sos_task: Unknown task error (%d)\n", err);
            }
            dprintf(6, "task_run_ready: task at %p is complete\n", task);
            DL_DELETE((*_ready), task);
            free(task);
            dprintf(7, "task_run_ready: task at %p freed\n", task);
        } // else SOS_TASK_YIELD - leave it in the queue
    };
    return;
}

void sos_task_run_high(void) {
    _sos_task_run_ready(&_high);
}

void sos_task_run_low(void) {
    _sos_task_run_ready(&_low);
}
